#include "Propulsion.h"
#include <iostream>
#include <vector>
#include <cmath>
using namespace std;
//Propulsion mission_file.moos app_name
double const Cthf = 1.5e-4;
double const Cthb = 1.5e-4;
double const Ctvf = 1.5e-4;
double const Ctvb = 1.5e-4;
double const R = 0.175;
void MiniROV_Propulsion(vector<double>& vdblControls, state& sState, vector<double>& vdblPropulsions){
	double speed_le, speed_ri, speed_vert;
	speed_le = vdblControls[0];
	speed_ri = vdblControls[1];
	speed_vert = vdblControls[2];
	
	double thrust_le, thrust_ri, thrust_vert;
	if(speed_le > 0){
		thrust_le = Cthf*abs(speed_le)*speed_le;
	}
	else{
		thrust_le = Cthb*abs(speed_le)*speed_le;
	}

	if(speed_ri > 0){
		thrust_ri = Cthf*abs(speed_ri)*speed_ri;
	}
	else{
		thrust_ri = Cthb*abs(speed_ri)*speed_ri;
	}
	
	if(speed_vert > 0){
		thrust_vert = Ctvf*abs(speed_vert)*speed_vert;
	}
	else{
		thrust_vert = Ctvb*abs(speed_vert)*speed_vert;
	}

	vdblPropulsions.push_back(thrust_le + thrust_ri);//tau1 - sila u X
	vdblPropulsions.push_back(0);//tau2
	vdblPropulsions.push_back(thrust_vert);//tau3 - sila u Z
	vdblPropulsions.push_back(0);//tau4
	vdblPropulsions.push_back(0);//tau5
	vdblPropulsions.push_back(R*(thrust_ri-thrust_le));//tau6 - sila u Y
}

int main(int argc, char* argv[]){
  const char* sMissionFile;
  const char* sMOOSName = "MiniROV_Propulsion";
  switch(argc)
  {
  case 3 :
         sMOOSName = argv[2];
  case 2 :
          sMissionFile = argv[1];
  }

  CPropulsion appPropulsion(MiniROV_Propulsion);
  appPropulsion.Run(sMOOSName, sMissionFile);
}

